////////////////////////////////////////////////////////////////////////////
// Copyright (c) 2012 Kentaro Sekimoto  All rights reserved.
////////////////////////////////////////////////////////////////////////////

#include <tinyhal.h>
#include "..\FM3.h"

//#define DEBUG_PWM
#define CQFM3DUINO  0
#define TARGET_BOARD    CQFM3DUINO

// TIOA Pin Table
static UINT8 tioa[] = {
    0x40, 0x41, 0x42, 0x43, 0x44, 0x45, 0xF3, 0x7C, // TIOA0_0 - 7_0
    0xA0, 0xA1, 0xA2, 0xA3, 0xA4, 0xA5, 0xC6, 0x78, // TIOA8_0 - 15_0
    0x3A, 0x3B, 0x3C, 0x3D, 0x3E, 0x3F, 0xC3, 0x23, // TIOA0_1 - 7_1
    0xF1, 0xB0, 0xB2, 0xB4, 0xB6, 0x5A, 0x7D, 0x7F, // TIOA8_1 - 15_1
    0x08, 0x53, 0x60, 0xD2, 0x70, 0x05, 0x5C, 0x76, // TIOA0_2 - 7_2
    0xC4, 0x56, 0xC5, 0x58, 0x36, 0x1A, 0x1C, 0x1E  // TIOA8_2 - 15_2
};

static UINT8 tioa_shift [] = {
    2, 10, 18, 26, 2, 10, 18, 26, 2, 10, 18, 26, 2, 10, 18, 26
};

INT8 FM3_PWM_PinToMode(UINT8 Pin)
{
    UINT32 i;
    INT8 mode = -1;
    for (i = 0; i < sizeof tioa; i++) {
        if (tioa[i] == Pin) {
            mode = i;
            break;
        }
    }
    return mode;
}

void FM3_PWM_SetMode(UINT32 mode)
{
    UINT32 port;
    UINT32 mask;
    UINT32 ch_num;
    UINT32 ch_mod;
    UINT32 ms, mr;

    port = GPIO_PORT(tioa[mode]);
    mask = GPIO_MASK(tioa[mode]);
    GPIO_PFR(port) |= mask;
    GPIO_PZR(port) |= mask;
    ch_mod = mode / 16;
    ch_num = mode % 16;
    ms = (ch_mod + 1) << tioa_shift[ch_num];
    mr = ~(0x3 << tioa_shift[ch_num]);
    if (ch_num < 4) {
        GPIO_EPFR(4) = (GPIO_EPFR(4) & mr) | ms;
    } else if (ch_num < 8) {
        GPIO_EPFR(5) = (GPIO_EPFR(5) & mr) | ms;
    } else if (ch_num < 12) {
        GPIO_EPFR(12) = (GPIO_EPFR(12) & mr) | ms;
    } else {
        GPIO_EPFR(13) = (GPIO_EPFR(13) & mr) | ms;
    }
}

struct FM3_PWM_Driver
{
#ifdef MB9BF618T
    static const UINT32 c_Channels = 8;
#else
#ifdef MB9BF506N
    static const UINT32 c_Channels = 6;
#else
    static const UINT32 c_Channels = 6;
#endif
#endif
//    void FM3_PWM_Driver();
    BOOL    Initialize        ( PWM_CHANNEL channel );
    BOOL    Uninitialize      ( PWM_CHANNEL channel );
#ifdef NETMF42_RTM
    BOOL    ApplyConfiguration( PWM_CHANNEL channel, GPIO_PIN pin, UINT32& period, UINT32& duration, BOOL invert );
#else
    BOOL    ApplyConfiguration( PWM_CHANNEL channel, GPIO_PIN pin, UINT32& period, UINT32& duration, PWM_SCALE_FACTOR& scale, BOOL invert );
#endif
    BOOL    Start             ( PWM_CHANNEL channel, GPIO_PIN pin );
    void    Stop              ( PWM_CHANNEL channel, GPIO_PIN pin );
    BOOL    Start             ( PWM_CHANNEL* channel, GPIO_PIN* pin, UINT32 count );
    void    Stop              ( PWM_CHANNEL* channel, GPIO_PIN* pin, UINT32 count );
    UINT32  Channels          ( );
    GPIO_PIN GetPinForChannel  ( PWM_CHANNEL channel );

    void EnablePin(PWM_CHANNEL channel);
    void DisablePin(PWM_CHANNEL channel);

    static const GPIO_PIN c_Pins[c_Channels];
    static const FM3_BT_PWM_TypeDef *c_PWM[c_Channels];
};

#ifdef MB9BF618T
#if (TARGET_BOARD == CQFM3DUINO)
const GPIO_PIN FM3_PWM_Driver::c_Pins[FM3_PWM_Driver::c_Channels] = {
    P3A,
    P3B,
    P3C,
    P3D,
    P3E,
    P3F,
    PF3,
    P23
};
#else
const GPIO_PIN FM3_PWM_Driver::c_Pins[FM3_PWM_Driver::c_Channels] = {
    P40,
    P41,
    P42,
    P43,
    P44,
    P45,
    PF3,
    P7C
};
#endif

const FM3_BT_PWM_TypeDef *FM3_PWM_Driver::c_PWM[c_Channels] = {
    (FM3_BT_PWM_TypeDef *)(FM3_PERIPH_BASE + 0x25000UL),
    (FM3_BT_PWM_TypeDef *)(FM3_PERIPH_BASE + 0x25040UL),
    (FM3_BT_PWM_TypeDef *)(FM3_PERIPH_BASE + 0x25080UL),
    (FM3_BT_PWM_TypeDef *)(FM3_PERIPH_BASE + 0x250C0UL),
    (FM3_BT_PWM_TypeDef *)(FM3_PERIPH_BASE + 0x25200UL),
    (FM3_BT_PWM_TypeDef *)(FM3_PERIPH_BASE + 0x25240UL),
    (FM3_BT_PWM_TypeDef *)(FM3_PERIPH_BASE + 0x25280UL),
    (FM3_BT_PWM_TypeDef *)(FM3_PERIPH_BASE + 0x252C0UL)
};
#else
#ifdef MB9BF506N
const GPIO_PIN FM3_PWM_Driver::c_Pins[FM3_PWM_Driver::c_Channels] = {
    P40,
    P41,
    P42,
    P43,
    P44,
    P45
};

const FM3_BT_PWM_TypeDef *FM3_PWM_Driver::c_PWM[c_Channels] = {
    (FM3_BT_PWM_TypeDef *)(FM3_PERIPH_BASE + 0x25000UL),
    (FM3_BT_PWM_TypeDef *)(FM3_PERIPH_BASE + 0x25040UL),
    (FM3_BT_PWM_TypeDef *)(FM3_PERIPH_BASE + 0x25080UL),
    (FM3_BT_PWM_TypeDef *)(FM3_PERIPH_BASE + 0x250C0UL),
    (FM3_BT_PWM_TypeDef *)(FM3_PERIPH_BASE + 0x25200UL),
    (FM3_BT_PWM_TypeDef *)(FM3_PERIPH_BASE + 0x25240UL),
};
#else
const GPIO_PIN FM3_PWM_Driver::c_Pins[FM3_PWM_Driver::c_Channels] = {
    P40,
    P41,
    P42,
    P43,
    P44,
    P45
};

const FM3_BT_PWM_TypeDef *FM3_PWM_Driver::c_PWM[c_Channels] = {
    (FM3_BT_PWM_TypeDef *)(FM3_PERIPH_BASE + 0x25000UL),
    (FM3_BT_PWM_TypeDef *)(FM3_PERIPH_BASE + 0x25040UL),
    (FM3_BT_PWM_TypeDef *)(FM3_PERIPH_BASE + 0x25080UL),
    (FM3_BT_PWM_TypeDef *)(FM3_PERIPH_BASE + 0x250C0UL),
    (FM3_BT_PWM_TypeDef *)(FM3_PERIPH_BASE + 0x25200UL),
    (FM3_BT_PWM_TypeDef *)(FM3_PERIPH_BASE + 0x25240UL),
};
#endif
#endif

FM3_PWM_Driver g_FM3_PWM_Driver;

//#if defined(ADS_LINKER_BUG__NOT_ALL_UNUSED_VARIABLES_ARE_REMOVED)
//#pragma arm section zidata
//#endif

BOOL PWM_Initialize(PWM_CHANNEL channel)
{
    return g_FM3_PWM_Driver.Initialize(channel);
}

BOOL PWM_Uninitialize(PWM_CHANNEL channel)
{
    return g_FM3_PWM_Driver.Uninitialize(channel);
}

#ifdef NETMF42_RTM
BOOL PWM_ApplyConfiguration(PWM_CHANNEL channel, GPIO_PIN pin, UINT32& period, UINT32& duration, BOOL invert)
#else
BOOL PWM_ApplyConfiguration(PWM_CHANNEL channel, GPIO_PIN pin, UINT32& period, UINT32& duration, PWM_SCALE_FACTOR& scale, BOOL invert)
#endif
{
    return g_FM3_PWM_Driver.ApplyConfiguration(channel, pin, period, duration, scale, invert);
}

BOOL PWM_Start(PWM_CHANNEL channel, GPIO_PIN pin)
{
    return g_FM3_PWM_Driver.Start(channel,pin);
}

void PWM_Stop(PWM_CHANNEL channel, GPIO_PIN pin)
{
    return g_FM3_PWM_Driver.Stop(channel,pin);
}

BOOL PWM_Start(PWM_CHANNEL* channel, GPIO_PIN* pin, UINT32 count)
{
    return g_FM3_PWM_Driver.Start(channel, pin, count);
}

void PWM_Stop(PWM_CHANNEL* channel, GPIO_PIN* pin, UINT32 count)
{
    return g_FM3_PWM_Driver.Stop(channel, pin, count);
}

UINT32 PWM_PWMChannels() 
{
    return g_FM3_PWM_Driver.Channels();
}

GPIO_PIN PWM_GetPinForChannel(PWM_CHANNEL channel)
{
    return g_FM3_PWM_Driver.GetPinForChannel(channel);
}

//--//

BOOL FM3_PWM_Driver::Initialize(PWM_CHANNEL channel)
{
    return TRUE;
}

BOOL FM3_PWM_Driver::Uninitialize(PWM_CHANNEL channel)
{
    return TRUE;
}

/*
 *  Register Bit Definition
 */
#define TMCR_STRG      0x0001U
#define TMCR_CTEN      0x0002U
#define TMCR_MDSE      0x0004U
#define TMCR_OSEL      0x0008U
#define TMCR_FMD_RST   0x0000U
#define TMCR_FMD_PWM   0x0010U
#define TMCR_EGS_DIS   0x0000U
#define TMCR_EGS_RISE  0x0100U
#define TMCR_EGS_FALL  0x0200U
#define TMCR_EGS_BOTH  0x0300U
#define TMCR_PMSK      0x0400U
#define TMCR_RTGEN     0x0800U
#define TMCR_CKS02_0   0x0000U
#define TMCR_CKS02_1   0x1000U
#define TMCR_CKS02_2   0x2000U
#define TMCR_CKS02_3   0x3000U
#define TMCR_CKS02_4   0x4000U
#define TMCR_CKS02_5   0x5000U
#define TMCR_CKS02_6   0x6000U
#define TMCR_CKS02_7   0x7000U

#define TMCR2_CKS3_0   0x00U
#define TMCR2_CKS3_1   0x01U

#define STC_UDIR       0x01U
#define STC_DTIR       0x02U
#define STC_TGIR       0x04U
#define STC_UDIE       0x10U
#define STC_DTIE       0x20U
#define STC_TGIE       0x40U
#define STC_IRQF       (STC_UDIR | STC_DTIR | STC_TGIR)

// TMCR
// b0: STRG
// b1: CTEN
// b2: MDSE
// b3: OSEL
// b4: FMD0
// b5: FMD1
// b6: FMD2
// b7: reserved
// b8: EGS0
// b9: EGS1
// b10:PMSK
// b11:RTGEN
// b12:CKS0
// b13:CKS1
// b14:CKS2
// b15 reserved
//#define DEBUG_PWM

UINT32 PWM_Clkdev_Get(PWM_SCALE_FACTOR& scale, UINT32 period)
{
    UINT32 clkdev;
#ifdef MB9BF618T
    if (scale == PWM_NANOSECONDS) {
        if (period < 100000)
            clkdev = 1;
        else if (period < 1000000)
            clkdev = 4;
        else if (period < 10000000)
            clkdev = 16;
        else if (period < 100000000)
            clkdev = 128;
        else
            clkdev = 2048;
    } else if (scale == PWM_MICROSECONDS) {
        if (period < 100)
            clkdev = 1;
        else if (period < 1000)
            clkdev = 4;
        else if (period < 10000)
            clkdev = 16;
        else if (period < 100000)
            clkdev = 128;
        else
            clkdev = 2048;
    } else {
        if (period < 10)
            clkdev = 16;
        else if (period < 100)
            clkdev = 128;
        else
            clkdev = 2048;
    }

#endif
#ifdef MB9BF506N
    if (scale == PWM_NANOSECONDS) {
        if (period < 1000000)
            clkdev = 1;
        else if (period < 10000000)
            clkdev = 16;
        else if (period < 100000000)
            clkdev = 64;
        else
            clkdev = 2048;
    } else if (scale == PWM_MICROSECONDS) {
        if (period < 1000)
            clkdev = 1;
        else if (period < 10000)
            clkdev = 16;
        else if (period < 100000)
            clkdev = 64;
        else
            clkdev = 2048;
    } else {
        if (period < 10)
            clkdev = 16;
        else if (period < 100)
            clkdev = 64;
        else
            clkdev = 2048;
    }
#endif
    return clkdev;
}

void PWM_Clock_Set(FM3_BT_PWM_TypeDef *pwm, UINT32 clkdev, BOOL invert)
{
    UINT16 cks;
    switch (clkdev) {
    case 4:
        cks = 1;
        break;
    case 16:
        cks = 2;
        break;
    case 128:
        cks = 3;
        break;
    case 256:
        cks = 4;
        break;
    case 512:
        cks = 8;
        break;
    case 1024:
        cks = 9;
        break;
    case 2048:
        cks = 10;
        break;
    case 1:
    default:
        cks = 0;
        break;
    }
    pwm->TMCR = 0x0810 + ((cks & 0x7) << 12);
    if (invert)
        pwm->TMCR |= 0x4;
    pwm->TMCR2 = (cks >> 3);
}

#ifdef NETMF42_RTM
BOOL FM3_PWM_Driver::ApplyConfiguration(PWM_CHANNEL channel, GPIO_PIN pin, UINT32& period, UINT32& duration, BOOL invert)
#else
BOOL FM3_PWM_Driver::ApplyConfiguration(PWM_CHANNEL channel, GPIO_PIN pin, UINT32& period, UINT32& duration, PWM_SCALE_FACTOR& scale, BOOL invert)
#endif
{
    UINT32 clkdev;
    //if (scale == PWM_NANOSECONDS) return FALSE;
    if ((UINT32)channel >= c_Channels)
        return FALSE;
    UINT32 flag_started = FALSE;
    //UINT32 period_ticks   = CPU_MicrosecondsToSystemClocks(period) / (PWM_CLKDEV * CLKDEV);
    //UINT32 duration_ticks = CPU_MicrosecondsToSystemClocks(duration) / (PWM_CLKDEV * CLKDEV);
    UINT32 period_ticks;
    UINT32 duration_ticks;
    FM3_BT_PWM_TypeDef *pwm = (FM3_BT_PWM_TypeDef *)c_PWM[(UINT32)channel];
    INT8 mode = FM3_PWM_PinToMode(c_Pins[(UINT8)channel]);
    if (mode < 0)
        return FALSE;
    FM3_PWM_SetMode(mode);
    if ((pwm->TMCR & TMCR_CTEN) != 0) {
        flag_started = TRUE;
        pwm->TMCR &= ~(TMCR_CTEN);
    }
    clkdev = PWM_Clkdev_Get(scale, period);
    PWM_Clock_Set(pwm, clkdev , invert);
    UINT32 ratio;
    if ((SYSTEM_CLOCK_HZ/2) < scale) {
        ratio = scale /(SYSTEM_CLOCK_HZ/2);
        period_ticks = (period / ratio) / clkdev;
        duration_ticks = (duration / ratio) / clkdev;
    } else {
        ratio = (SYSTEM_CLOCK_HZ/2) / scale;
        period_ticks = (period * ratio) / clkdev;
        duration_ticks = (duration * ratio) / clkdev;
    }
#ifdef DEBUG_PWM
    lcd_printf("P=%08x C=%d S=%d    \r\n", period, clkdev, scale);
    lcd_printf("p=%08x d=%08x\r\n", period_ticks, duration_ticks);
#endif
    if (period_ticks == 0 || period_ticks > 0xffff) {
        lcd_printf("Unsuppored Freq!\r\n");
        return FALSE;
    }
    pwm->STC = 0;
    pwm->PCSR = (UINT16)period_ticks;
    pwm->PDUT = (UINT16)duration_ticks;
    if (flag_started == TRUE) {
        pwm->TMCR |= (TMCR_CTEN | TMCR_STRG);
    }
    return TRUE;
}

void FM3_PWM_Driver::EnablePin(PWM_CHANNEL channel)
{
//    CPU_GPIO_DisablePin(g_FM3_PWM_Driver.c_Pins[channel], RESISTOR_DISABLED, 0, GPIO_ALT_MODE_2);
    FM3_BT_PWM_TypeDef *pwm = (FM3_BT_PWM_TypeDef *)c_PWM[(UINT32)channel];
    pwm->TMCR |= (TMCR_CTEN | TMCR_STRG);
}

void FM3_PWM_Driver::DisablePin(PWM_CHANNEL channel)
{
    FM3_BT_PWM_TypeDef *pwm = (FM3_BT_PWM_TypeDef *)c_PWM[(UINT32)channel];
    pwm->TMCR &= ~(TMCR_CTEN | TMCR_STRG);
//    CPU_GPIO_DisablePin(g_FM3_PWM_Driver.c_Pins[channel], RESISTOR_PULLDOWN, 0, GPIO_ALT_PRIMARY);
}

BOOL FM3_PWM_Driver::Start(PWM_CHANNEL channel, GPIO_PIN pin)
{
    EnablePin(channel);
    return TRUE;
}

void FM3_PWM_Driver::Stop(PWM_CHANNEL channel, GPIO_PIN pin)
{    
    DisablePin(channel);
}

BOOL FM3_PWM_Driver::Start(PWM_CHANNEL* channel, GPIO_PIN* pin, UINT32 count)
{
    return TRUE;
}

void FM3_PWM_Driver::Stop(PWM_CHANNEL* channel, GPIO_PIN* pin, UINT32 count)
{
}

UINT32 FM3_PWM_Driver::Channels()
{
    return c_Channels;
}

GPIO_PIN FM3_PWM_Driver::GetPinForChannel(PWM_CHANNEL channel)
{
    if ((UINT32)channel >= c_Channels)
        return GPIO_PIN_NONE;
    return (GPIO_PIN)g_FM3_PWM_Driver.c_Pins[channel];
}
